/** \file gr_aligned_allocator.h Provides simd aligned memory (16 bytes)*/

#pragma once

#include "gr_algorithms.h"

#include<cstring>
#include<algorithm>

namespace gr{

    /** Return memory aligned to 16 bytes. Must be freed using alignedFree */
    unsigned char* alignedAlloc(size_t size);
    void alignedFree(void* p);

    /** Use the aligned allocator to store types of proper alignment. TODO: Check alignment of value type! */
    template<class T>
    class AlignedArray
    {
    public:
        typedef T  value_type;
        typedef T* iterator;

        AlignedArray(size_t hint = 12):
            data_(nullptr), capacity_(0), size_(0)
        {
            reallocData(hint);
        }

        AlignedArray(const AlignedArray& old)
        {
            *this = old;
        }

        ~AlignedArray()
        {
            if(data_) alignedFree(data_);
        }

        AlignedArray& operator=(const AlignedArray& old)
        {
            reallocData(old.capacity());
            unsafeCopy(old.begin(), old.end(), data_);
            size_ = old.size();
        }

        T& operator[](size_t index)
        {
            return data_[index];
        }

        T& append()
        {
            addData(1);
            return &data_[size_ - 1];
        }

        void resize(size_t newSize, const T& initValue)
        {
            reallocData(newSize);
            if(newSize > size_)
            {
                size_t delta = newSize - size_;
                size_t oldSize = size_;
                addData(delta);
                for(int i = oldSize; i < newSize; ++i) data_[i] = initValue;
            }
        }

        size_t size() const {return size_;}

        iterator begin()
        {
            return data_;
        }

        iterator end()
        {
            return data_ + size_;
        }

        size_t capacity()
        {
            return capacity_;
        }

        size_t valueSize()
        {
            return sizeof(T);
        }

        void assign(T* startPtr, T* endPtr)
        {
            size_t count = endPtr - startPtr;
            size_ = 0;
            addData(count);
            unsafeCopy(startPtr, endPtr, data_);
        }
        void clear()
        {
            size_  = 0;
        }
        size_t size()
        {
            return size_;
        }

    private:

        void addData(size_t count)
        {
            size_t newSize = size_ + count;
            if(newSize > capacity_)
            {
                auto newCapacity = capacity_ * 2;
                if(newSize > newCapacity) newCapacity = newSize + newSize / 5;

                reallocData(newCapacity);
            }

            size_ = newSize;
        }

        // TODO add safety checks
        void reallocData(size_t newCapacity)
        {
            if(newCapacity <= capacity_) return;

            size_t newSizeBytes =  countInBytes(newCapacity);

            if(data_)
            {
                T* newData = alignedAlloc(newSizeBytes);
                unsafeCopy(begin(), end(), newData);
                alignedFree(data_);
                data_ = newData;
            }
            else
            {
                data_ = alignedAlloc(newSizeBytes);
                memset(data_, 0, newSizeBytes);
            }

            capacity_ = newCapacity;
        }

        size_t countInBytes(size_t count){return count * valueSize();}

        T* data_;
        size_t capacity_;
        size_t size_;
    };
}
